/*
 * control.hpp
 *
 *  Created on: 2020年9月2日
 *      Author: 17900
 */

#ifndef CONTROL_POSHOLD_HPP_
#define CONTROL_POSHOLD_HPP_

#ifdef __cplusplus
extern "C"
{
#endif

#include "math.h"
#ifdef __cplusplus
}
#endif


#ifdef __cplusplus

#include "userlib/userlib.hpp"
#include "pid/pid.hpp"
#include "control/control_attitude.hpp"
#include "trajectory/trajectory.hpp"

#define COLL_AMPLIFY 30
#define G_zinit 9.8f
//void EXTI15_10_IRQHandler(void);

typedef struct limits
{
	float ProErrorLimits[2];
	float VelErrorLimits[2];
	float IntStateLimits[2];
	float OutErrorLimits[2];
	float int_state;
} sLIMITS;




class CONTROL_POSHOLD:public CONTROL_ALTITUDE
{
public:
	CONTROL_POSHOLD(){}
	~CONTROL_POSHOLD(){}
	void Heli_Poshold_control(void);
	void Heli_Speed_control(void);

	void Pos_NED_Yaw_Command(void);
	void Pos_Vel_NED_Feedback(void);
	void Pos_Vel_NED_Loop(void);
	void RC_Vel_NED_Loop(void);
	void Vel_Acc_NED_Loop(void);
	void Acc_NE_Roll_Pitch_Loop(void);
	void Altitude_Acc_Loop(void);
	void PosHold_Control_Step(void);
	void Speed_Control_Step(void);
	void Pos_Vel_Loop_Init(void);
	void Pos_Hold_Init(void);
	void Speed_Hold_Init(void);
	//void Trajectory_Target_Receive(void);
	void Set_Limits(void);

private:
	sLIMITS N_limits,E_limits,D_limits,Vel_N_limits,Vel_E_limits,Vel_D_limits,roll_limits,pitch_limits,yaw_limits,coll_limits,roll_rate_limits,pitch_rate_limits;

};


#endif



#endif /* CONTROL_HPP_ */
